#include "LBFGS.h"

//vector<double>* LBFGSopt::g_pDose = NULL;
OptData* LBFGSopt::g_optData = NULL;
vector<SConstraint>* LBFGSopt::g_pConstraints = NULL;
vector<double>* LBFGSopt::g_objFunValue = NULL;

LBFGSopt* LBFGSopt::Instance() {

    static LBFGSopt instance;
    return &instance;
}

void LBFGSopt::function_grad(const real_1d_array &x, double &func, real_1d_array &grad, void *ptr) {
    //long step1, step2, step3, step4,step5, step6, step7, step8;
    //step1 = clock();

    func = 0;
    for (int i = 0; i != x.length(); i++) {
        grad[i] = 0;
    }
    //step2 = clock();
    vector<double> *tempWeight = new vector<double>(x.length());
    for (int indexResult = 0; indexResult != x.length(); indexResult++) {
        (*tempWeight)[indexResult] = x[indexResult] * x[indexResult];
    }
    //step3=clock();
    //LBFGSopt::g_pDose = new vector<double>;
    LBFGSopt::g_optData->calculateDose(tempWeight);
    //step4=clock();
    vector<SConstraint>::iterator iterConstraint;
    for (iterConstraint = LBFGSopt::g_pConstraints->begin(); iterConstraint != LBFGSopt::g_pConstraints->end(); iterConstraint++) {
        //step5 = clock();
        //initialization
        string strStructureName = iterConstraint->strStructureName;
        double aimDose = iterConstraint->aimDose;
        double aimWeight = iterConstraint->aimWeight;
        string constraintType = iterConstraint->constraintType;
        vector<vector<double> > *vvDij = LBFGSopt::g_optData->getDijTotal();

        vector<int> *aimStructure = NULL;
        for (int indexStr = 0; indexStr != LBFGSopt::g_optData->getStructures()->size(); indexStr++) {
            if ((*LBFGSopt::g_optData->getStructures())[indexStr].getStructName() == strStructureName) {
                aimStructure = (*LBFGSopt::g_optData->getStructures())[indexStr].getPoints();
            }
        }
        if (aimStructure == NULL) {
            cerr << "structure error" << endl;
        }
        //step6 = clock();
        //create func and grad[]
        double tempTotleFunc = 0;
        for (int indexPoint = 0; indexPoint != aimStructure->size(); indexPoint++) {
            double temp = 0;
            double contri = 0;
            double wei = 0;
            unsigned int position = (*aimStructure)[indexPoint];

            temp = (*LBFGSopt::g_optData->getDose())[position];

            if ((constraintType == "upper")&&(temp > aimDose)) {
                tempTotleFunc += (temp - aimDose)*(temp - aimDose);
            } else if ((constraintType == "lower")&&(temp < aimDose)) {
                tempTotleFunc += (temp - aimDose)*(temp - aimDose);
            } else if ((constraintType == "uniform")) {
                tempTotleFunc += (temp - aimDose)*(temp - aimDose);
            }
        }
        func += (tempTotleFunc / double(aimStructure->size())) * aimWeight;

        //step7=clock();
        //calculate grad[i]
        for (unsigned int j = 0; j != x.length(); j++) {
            double temp = 0;
            double temp2 = 0;
            double tempTotalGrad = 0;

            for (int indexPoint = 0; indexPoint != aimStructure->size(); indexPoint++) {
                temp = 0;
                temp2 = 0;
                double contri = 0;
                double wei = 0;
                unsigned int position = (*aimStructure)[indexPoint];

                contri = (*vvDij)[position][j];
                if (contri > 0.0000000001) {
                    temp2 = contri * x[j];

                    temp = (*LBFGSopt::g_optData->getDose())[position];

                    if ((constraintType == "upper")&&(temp > aimDose)) {
                        tempTotalGrad += 4 * (temp - aimDose) * temp2;
                    } else if ((constraintType == "lower")&&(temp < aimDose)) {
                        tempTotalGrad += 4 * (temp - aimDose) * temp2;
                    } else if ((constraintType == "uniform")) {
                        tempTotalGrad += 4 * (temp - aimDose) * temp2;
                    }
                }
            }
            grad[j] += (tempTotalGrad / double(aimStructure->size())) * aimWeight;

        }
        //step8=clock();
    }

    cout << func << endl;
    g_objFunValue->push_back(func);
    //cout<<step2-step1<<"\t"<<step3-step2<<"\t"<<step4-step3<<"\t"<<step5-step4<<"\t"<<step6-step5<<"\t"<<step7-step6<<"\t"<<step8-step7<<endl;
    //delete LBFGSopt::g_pDose;
}

void LBFGSopt::optimization(vector<double> *resutlWeight, OptData* optData, vector<SConstraint> *pConstrain, vector<double> *objFunValue, int maxIter) {
    LBFGSopt::g_optData = optData;
    LBFGSopt::g_pConstraints = pConstrain;
    LBFGSopt::g_objFunValue = objFunValue;

    unsigned int nWeightLen = LBFGSopt::g_optData->getWeightTotal()->size();
    //size_t xLen = g_pDoseMatrix->m_vWeightUni->size();
    double *xArray = new double[nWeightLen];
    for (int indexWei = 0; indexWei != nWeightLen; indexWei++) {
        xArray[indexWei] = sqrt((*LBFGSopt::g_optData->getWeightTotal())[indexWei]);
    }

    real_1d_array x;
    x.setcontent(nWeightLen, xArray);
    delete xArray;

    double epsg = 0.0000000001;
    double epsf = 0;
    double epsx = 0;
    ae_int_t maxits = maxIter;
    minlbfgsstate state;
    minlbfgsreport rep;

    minlbfgscreate(1, x, state);
    minlbfgssetcond(state, epsg, epsf, epsx, maxits);

    alglib::minlbfgsoptimize(state, LBFGSopt::function_grad);

    minlbfgsresults(state, x, rep);

    for (int i = 0; i < x.length(); i++) {
        resutlWeight->push_back(x[i] * x[i]);
    }
}

void LBFGSopt::function_grad_modify_1(const real_1d_array &x, double &func, real_1d_array &grad, void *ptr) {
    //long step1, step2, step3, step4,step5, step6, step7, step8;
    //step1 = clock();

    func = 0;
    for (int i = 0; i != x.length(); i++) {
        grad[i] = 0;
    }
    //step2 = clock();
    vector<double> *tempWeight = new vector<double>(x.length());
    for (int indexResult = 0; indexResult != x.length(); indexResult++) {
        (*tempWeight)[indexResult] = x[indexResult] * x[indexResult];
    }
    //step3=clock();
    //LBFGSopt::g_pDose = new vector<double>;
    LBFGSopt::g_optData->calculateDose(tempWeight);
    //step4=clock();
    vector<SConstraint>::iterator iterConstraint;
    for (iterConstraint = LBFGSopt::g_pConstraints->begin(); iterConstraint != LBFGSopt::g_pConstraints->end(); iterConstraint++) {
        //step5 = clock();
        //initialization
        string strStructureName = iterConstraint->strStructureName;
        double aimDose = iterConstraint->aimDose;
        double aimWeight = iterConstraint->aimWeight;
        string constraintType = iterConstraint->constraintType;
        vector<vector<double> > *vvDij = LBFGSopt::g_optData->getDijTotal();

        vector<int> *aimStructure = NULL;
        for (int indexStr = 0; indexStr != LBFGSopt::g_optData->getStructures()->size(); indexStr++) {
            if ((*LBFGSopt::g_optData->getStructures())[indexStr].getStructName() == strStructureName) {
                aimStructure = (*LBFGSopt::g_optData->getStructures())[indexStr].getPoints();
            }
        }
        if (aimStructure == NULL) {
            cerr << "structure error" << endl;
        }
        //step6 = clock();
        //create func and grad[]
        double tempTotleFunc = 0;
        for (int indexPoint = 0; indexPoint != aimStructure->size(); indexPoint++) {
            double temp = 0;
            double contri = 0;
            double wei = 0;
            unsigned int position = (*aimStructure)[indexPoint];

            temp = (*LBFGSopt::g_optData->getDose())[position];

            if ((constraintType == "upper")&&(temp > aimDose)) {
                tempTotleFunc += (temp - aimDose)*(temp - aimDose);
            } else if ((constraintType == "lower")&&(temp < aimDose)) {
                tempTotleFunc += (temp - aimDose)*(temp - aimDose);
            } else if ((constraintType == "uniform")) {
                tempTotleFunc += (temp - aimDose)*(temp - aimDose);
            }
        }
        func += (tempTotleFunc / double(aimStructure->size())) * aimWeight;

        //step7=clock();
        //calculate grad[i]
        for (unsigned int j = 0; j != x.length(); j++) {
            double temp = 0;
            double temp2 = 0;
            double tempTotalGrad = 0;

            for (int indexPoint = 0; indexPoint != aimStructure->size(); indexPoint++) {
                temp = 0;
                temp2 = 0;
                double contri = 0;
                double wei = 0;
                unsigned int position = (*aimStructure)[indexPoint];

                if ((*vvDij)[position].size() > j) {
                    contri = (*vvDij)[position][j];
                }

                if (contri > 0.0000000001) {
                    temp2 = contri * x[j];

                    temp = (*LBFGSopt::g_optData->getDose())[position];

                    if ((constraintType == "upper")&&(temp > aimDose)) {
                        tempTotalGrad += 4 * (temp - aimDose) * temp2;
                    } else if ((constraintType == "lower")&&(temp < aimDose)) {
                        tempTotalGrad += 4 * (temp - aimDose) * temp2;
                    } else if ((constraintType == "uniform")) {
                        tempTotalGrad += 4 * (temp - aimDose) * temp2;
                    }
                }
            }
            grad[j] += (tempTotalGrad / double(aimStructure->size())) * aimWeight;

        }
        //step8=clock();
    }

    cout << func << endl;
    g_objFunValue->push_back(func);
    //cout<<step2-step1<<"\t"<<step3-step2<<"\t"<<step4-step3<<"\t"<<step5-step4<<"\t"<<step6-step5<<"\t"<<step7-step6<<"\t"<<step8-step7<<endl;
    //delete LBFGSopt::g_pDose;
}

void LBFGSopt::optimization_modify_1(vector<double> *resutlWeight, OptData* optData, vector<SConstraint> *pConstrain, vector<double> *objFunValue, int maxIter) {
    LBFGSopt::g_optData = optData;
    LBFGSopt::g_pConstraints = pConstrain;
    LBFGSopt::g_objFunValue = objFunValue;

    unsigned int nWeightLen = LBFGSopt::g_optData->getWeightTotal()->size();
    //size_t xLen = g_pDoseMatrix->m_vWeightUni->size();
    //initialize weight
    //make sure all beamlets used in optimization are != 0
    //since if beamlet weight == 0, it will always == 0 
    vector<double> tempWeight;
    tempWeight.resize(nWeightLen, 0);
    for (int i = 0; i < optData->getDijTotal()->size(); i++) {
        for (int j = 0; j < (*optData->getDijTotal())[i].size(); j++) {
            if ((*optData->getDijTotal())[i][j] > 0) {
                tempWeight[j] += (*optData->getDijTotal())[i][j];
            }
        }
    }


    double *xArray = new double[nWeightLen];
    for (int indexWei = 0; indexWei != nWeightLen; indexWei++) {
        if ((*LBFGSopt::g_optData->getWeightTotal())[indexWei] > 0) {
            xArray[indexWei] = sqrt((*LBFGSopt::g_optData->getWeightTotal())[indexWei]);
        } else if (tempWeight[indexWei] > 0) {
            xArray[indexWei] = 0.00001;
        }

    }

    real_1d_array x;
    x.setcontent(nWeightLen, xArray);
    delete xArray;

    double epsg = 0.0000000001;
    double epsf = 0;
    double epsx = 0;
    ae_int_t maxits = maxIter;
    minlbfgsstate state;
    minlbfgsreport rep;

    minlbfgscreate(1, x, state);
    minlbfgssetcond(state, epsg, epsf, epsx, maxits);

    alglib::minlbfgsoptimize(state, LBFGSopt::function_grad_modify_1);

    minlbfgsresults(state, x, rep);

    for (int i = 0; i < x.length(); i++) {
        resutlWeight->push_back(x[i] * x[i]);
    }
}

void LBFGSopt::function_grad_modify_2(const real_1d_array &x, double &func, real_1d_array &grad, void *ptr) {
    //long step1, step2, step3, step4,step5, step6, step7, step8;
    //step1 = clock();

    func = 0;
    for (int i = 0; i != x.length(); i++) {
        grad[i] = 0;
    }
    //step2 = clock();
    vector<double> *tempWeight = new vector<double>(x.length());
    for (int indexResult = 0; indexResult != x.length(); indexResult++) {
        if (x[indexResult] <= 0) {
            (*tempWeight)[indexResult] = 0;
        } else {
            (*tempWeight)[indexResult] = x[indexResult];
        }

    }
    //step3=clock();
    //LBFGSopt::g_pDose = new vector<double>;
    LBFGSopt::g_optData->calculateDose(tempWeight);
    //step4=clock();
    vector<SConstraint>::iterator iterConstraint;
    for (iterConstraint = LBFGSopt::g_pConstraints->begin(); iterConstraint != LBFGSopt::g_pConstraints->end(); iterConstraint++) {
        //step5 = clock();
        //initialization
        string strStructureName = iterConstraint->strStructureName;
        double aimDose = iterConstraint->aimDose;
        double aimWeight = iterConstraint->aimWeight;
        string constraintType = iterConstraint->constraintType;
        vector<vector<double> > *vvDij = LBFGSopt::g_optData->getDijTotal();

        vector<int> *aimStructure = NULL;
        for (int indexStr = 0; indexStr != LBFGSopt::g_optData->getStructures()->size(); indexStr++) {
            if ((*LBFGSopt::g_optData->getStructures())[indexStr].getStructName() == strStructureName) {
                aimStructure = (*LBFGSopt::g_optData->getStructures())[indexStr].getPoints();
            }
        }
        if (aimStructure == NULL) {
            cerr << "structure error" << endl;
        }
        //step6 = clock();
        //create func and grad[]
        double tempTotleFunc = 0;
        for (int indexPoint = 0; indexPoint != aimStructure->size(); indexPoint++) {
            double temp = 0;
            double contri = 0;
            double wei = 0;
            unsigned int position = (*aimStructure)[indexPoint];

            temp = (*LBFGSopt::g_optData->getDose())[position];

            if ((constraintType == "upper")&&(temp > aimDose)) {
                tempTotleFunc += (temp - aimDose)*(temp - aimDose);
            } else if ((constraintType == "lower")&&(temp < aimDose)) {
                tempTotleFunc += (temp - aimDose)*(temp - aimDose);
            } else if ((constraintType == "uniform")) {
                tempTotleFunc += (temp - aimDose)*(temp - aimDose);
            }
        }
        func += (tempTotleFunc / double(aimStructure->size())) * aimWeight;

        //step7=clock();
        //calculate grad[i]
        for (unsigned int j = 0; j != x.length(); j++) {
            double temp = 0;
            double temp2 = 0;
            double tempTotalGrad = 0;

            for (int indexPoint = 0; indexPoint != aimStructure->size(); indexPoint++) {
                temp = 0;
                temp2 = 0;
                double contri = 0;
                double wei = 0;
                unsigned int position = (*aimStructure)[indexPoint];

                if ((*vvDij)[position].size() > j) {
                    contri = (*vvDij)[position][j];
                }

                if (contri > 0.0000000001) {
                    temp2 = contri;

                    temp = (*LBFGSopt::g_optData->getDose())[position];

                    if ((constraintType == "upper")&&(temp > aimDose)) {
                        tempTotalGrad += 2 * (temp - aimDose) * temp2;
                    } else if ((constraintType == "lower")&&(temp < aimDose)) {
                        tempTotalGrad += 2 * (temp - aimDose) * temp2;
                    } else if ((constraintType == "uniform")) {
                        tempTotalGrad += 2 * (temp - aimDose) * temp2;
                    }
                }
            }
            grad[j] += (tempTotalGrad / double(aimStructure->size())) * aimWeight;

        }
        //step8=clock();
    }

    cout << func << endl;
    g_objFunValue->push_back(func);
    //cout<<step2-step1<<"\t"<<step3-step2<<"\t"<<step4-step3<<"\t"<<step5-step4<<"\t"<<step6-step5<<"\t"<<step7-step6<<"\t"<<step8-step7<<endl;
    //delete LBFGSopt::g_pDose;
}

void LBFGSopt::optimization_modify_2(vector<double> *resutlWeight, OptData* optData, vector<SConstraint> *pConstrain, vector<double> *objFunValue, int maxIter) {
    LBFGSopt::g_optData = optData;
    LBFGSopt::g_pConstraints = pConstrain;
    LBFGSopt::g_objFunValue = objFunValue;

    unsigned int nWeightLen = LBFGSopt::g_optData->getWeightTotal()->size();
    //size_t xLen = g_pDoseMatrix->m_vWeightUni->size();
    double *xArray = new double[nWeightLen];
    for (int indexWei = 0; indexWei != nWeightLen; indexWei++) {
        xArray[indexWei] = (*LBFGSopt::g_optData->getWeightTotal())[indexWei];
    }

    real_1d_array x;
    x.setcontent(nWeightLen, xArray);
    delete xArray;

    double epsg = 0.0000000001;
    double epsf = 0;
    double epsx = 0;
    ae_int_t maxits = maxIter;
    minlbfgsstate state;
    minlbfgsreport rep;

    minlbfgscreate(1, x, state);
    minlbfgssetcond(state, epsg, epsf, epsx, maxits);

    alglib::minlbfgsoptimize(state, LBFGSopt::function_grad_modify_2);

    minlbfgsresults(state, x, rep);

    for (int i = 0; i < x.length(); i++) {
        if (x[i] >= 0) {
            resutlWeight->push_back(x[i]);
        } else {
            resutlWeight->push_back(0);
        }

    }
}

void LBFGSopt::function_grad_EUD(const real_1d_array &x, double &func, real_1d_array &grad, void *ptr) {

    func = 0;
#ifdef USE_OMP
#pragma omp parallel for
#endif
    for (int i = 0; i < x.length(); i++) {
        grad[i] = 0;
    }

    vector<double> *tempWeight = new vector<double>(x.length());
#ifdef USE_OMP
#pragma omp parallel for
#endif
    for (int indexResult = 0; indexResult < x.length(); indexResult++) {
        (*tempWeight)[indexResult] = pow(x[indexResult], 2);
    }

    LBFGSopt::g_optData->calculateDose(tempWeight);

    LBFGSopt::g_optData->calculateEUD();

    //target uniform penalty
    {
        SConstraint targetCnst = (*LBFGSopt::g_pConstraints)[0];

        string strStructureName = targetCnst.strStructureName;
        double aimDose = targetCnst.aimDose;
        double aimWeight = targetCnst.aimWeight;
        string constraintType = targetCnst.constraintType;
        vector<vector<double> > *vvDij = LBFGSopt::g_optData->getDijTotal();

        vector<int> *aimStructure = NULL;

        for (int indexStr = 0; indexStr < LBFGSopt::g_optData->getStructures()->size(); indexStr++) {
            if ((*LBFGSopt::g_optData->getStructures())[indexStr].getStructName() == strStructureName) {
                aimStructure = (*LBFGSopt::g_optData->getStructures())[indexStr].getPoints();
            }
        }
        if (aimStructure == NULL) {
            cerr << "structure error" << endl;
        }

        double tempTotalFunc = 0;
        int indexPoint = 0;
#ifdef USE_OMP
#pragma omp parallel for default(shared) private(indexPoint) reduction(+:tempTotalFunc)
#endif
        for (indexPoint = 0; indexPoint < aimStructure->size(); indexPoint++) {
            double temp = 0;
            double contri = 0;
            double wei = 0;
            double tempFunc = 0;
            unsigned int position = (*aimStructure)[indexPoint];

            temp = (*LBFGSopt::g_optData->getDose())[position];

            if ((constraintType == "upper")&&(temp > aimDose)) {
                tempFunc = (temp - aimDose)*(temp - aimDose);
            } else if ((constraintType == "lower")&&(temp < aimDose)) {
                tempFunc = (temp - aimDose)*(temp - aimDose);
            } else if ((constraintType == "uniform")) {
                tempFunc = (temp - aimDose)*(temp - aimDose);
            }

            tempTotalFunc += tempFunc;
        }

        func += (tempTotalFunc / double(aimStructure->size())) * aimWeight;

        int j = 0;
#ifdef USE_OMP
#pragma omp parallel for default(shared) private(j,indexPoint) schedule(dynamic)
#endif
        for (j = 0; j < x.length(); j++) {
            double tempTotalGrad = 0;

            for (indexPoint = 0; indexPoint < aimStructure->size(); indexPoint++) {
                double temp = 0;
                double temp2 = 0;
                double contri = 0;
                double wei = 0;
                unsigned int position = (*aimStructure)[indexPoint];

                if ((*vvDij)[position].size() > j) {
                    contri = (*vvDij)[position][j];
                }

                if (contri > 0.0000000001) {
                    temp2 = contri * x[j];

                    temp = (*LBFGSopt::g_optData->getDose())[position];

                    if ((constraintType == "upper")&&(temp > aimDose)) {
                        tempTotalGrad += 4 * (temp - aimDose) * temp2;
                    } else if ((constraintType == "lower")&&(temp < aimDose)) {
                        tempTotalGrad += 4 * (temp - aimDose) * temp2;
                    } else if ((constraintType == "uniform")) {
                        tempTotalGrad += 4 * (temp - aimDose) * temp2;
                    }
                }
            }
            grad[j] += (tempTotalGrad / double(aimStructure->size())) * aimWeight;
        }
    }

    //for OAR EUD constraints
    for (int indexStruc = 1; indexStruc < (*LBFGSopt::g_pConstraints).size(); indexStruc++) {
        SConstraint targetCnst = (*LBFGSopt::g_pConstraints)[indexStruc];
        //step5 = clock();
        //initialization
        string strStructureName = targetCnst.strStructureName;
        double aimDose = targetCnst.aimDose;
        double aimWeight = targetCnst.aimWeight;
        string constraintType = targetCnst.constraintType;
        vector<vector<double> > *vvDij = LBFGSopt::g_optData->getDijTotal();

        vector<int> *aimStructure = NULL;
        double aimEUD = 0;
        double a = 0;
        for (int indexStr = 0; indexStr != LBFGSopt::g_optData->getStructures()->size(); indexStr++) {
            if ((*LBFGSopt::g_optData->getStructures())[indexStr].getStructName() == strStructureName) {
                aimStructure = (*LBFGSopt::g_optData->getStructures())[indexStr].getPoints();
                aimEUD = (*LBFGSopt::g_optData->getEUD())[indexStr];

                a = (*LBFGSopt::g_optData->getStructures())[indexStr].getEUDfac();

            }
        }
        if (aimStructure == NULL) {
            cerr << "structure error" << endl;
        }
        //step6 = clock();
        //create func and grad[]
        //func += (aimEUD*aimEUD)*aimWeight;
        func += aimEUD*aimWeight;

        double sumPowDose = 0; //sigma Di^a
        int indexPoint = 0;
#ifdef USE_OMP
#pragma omp parallel for default(shared) reduction(+:sumPowDose) private(indexPoint)
#endif
        for (indexPoint = 0; indexPoint < aimStructure->size(); indexPoint++) {
            unsigned int position = (*aimStructure)[indexPoint];
            sumPowDose += pow((*LBFGSopt::g_optData->getDose())[position], a);
        }
        //cout<<sumPowDose<<endl;
        //calculate grad[i]

        int j = 0;
#ifdef USE_OMP
#pragma omp parallel for default(shared) private(j) schedule(dynamic)
#endif
        for (j = 0; j < x.length(); j++) {
            double temp = 0;
            double temp2 = 0;
            double tempTotalGrad = 0;

            for (int indexPoint = 0; indexPoint != aimStructure->size(); indexPoint++) {
                temp = 0;
                temp2 = 0;
                double contri = 0;
                double wei = 0;
                unsigned int position = (*aimStructure)[indexPoint];

                if ((*vvDij)[position].size() > j) {
                    contri = (*vvDij)[position][j];
                }

                if (contri > 0.0000000001) {
                    temp2 = 2 * contri * x[j];
                    temp = pow((*LBFGSopt::g_optData->getDose())[position], a - 1);
                    //tempTotalGrad += 2*aimEUD*aimEUD*temp/sumPowDose*temp2;

                    tempTotalGrad += aimEUD * temp / sumPowDose*temp2;

                }
            }
            //cout<<tempTotalGrad<<" ";
            grad[j] += (tempTotalGrad) * aimWeight;

        }
    }

    cout << func << endl;
    g_objFunValue->push_back(func);
}

void LBFGSopt::function_grad_EUD_new(const real_1d_array &x, double &func, real_1d_array &grad, void *ptr) {

    func = 0;
#ifdef USE_OMP
#pragma omp parallel for
#endif
    for (int i = 0; i < x.length(); i++) {
        grad[i] = 0;
    }

    vector<double> *tempWeight = new vector<double>(x.length());
#ifdef USE_OMP
#pragma omp parallel for
#endif
    for (int indexResult = 0; indexResult < x.length(); indexResult++) {
        (*tempWeight)[indexResult] = pow(x[indexResult], 2);
    }

    LBFGSopt::g_optData->calculateDose(tempWeight);

    LBFGSopt::g_optData->calculateEUD();

    for (int indexStruc = 0; indexStruc < (*LBFGSopt::g_pConstraints).size(); indexStruc++) {
        SConstraint targetCnst = (*LBFGSopt::g_pConstraints)[indexStruc];

        string strStructureName = targetCnst.strStructureName;
        double aimDose = targetCnst.aimDose;
        double aimWeight = targetCnst.aimWeight;
        string constraintType = targetCnst.constraintType;
        vector<vector<double> > *vvDij = LBFGSopt::g_optData->getDijTotal();

        vector<int> *aimStructure = NULL;

        double aimEUD = 0;
        double a = 0;
        for (int indexStr = 0; indexStr != LBFGSopt::g_optData->getStructures()->size(); indexStr++) {
            if ((*LBFGSopt::g_optData->getStructures())[indexStr].getStructName() == strStructureName) {
                aimStructure = (*LBFGSopt::g_optData->getStructures())[indexStr].getPoints();
                aimEUD = (*LBFGSopt::g_optData->getEUD())[indexStr]; //the value of EUD

                a = (*LBFGSopt::g_optData->getStructures())[indexStr].getEUDfac(); //EUDfactor

            }
        }
        if (aimStructure == NULL) {
            cerr << "structure error" << endl;
        }



        if (constraintType == "EUD") {//for EUD
            func += aimEUD*aimWeight;

            double sumPowDose = 0; //sigma Di^a
            int indexPoint = 0;
#ifdef USE_OMP
#pragma omp parallel for default(shared) reduction(+:sumPowDose) private(indexPoint)
#endif
            for (indexPoint = 0; indexPoint < aimStructure->size(); indexPoint++) {
                unsigned int position = (*aimStructure)[indexPoint];
                sumPowDose += pow((*LBFGSopt::g_optData->getDose())[position], a);
            }
            //cout<<sumPowDose<<endl;
            //calculate grad[i]

            int j = 0;
#ifdef USE_OMP
#pragma omp parallel for default(shared) private(j) schedule(dynamic)
#endif
            for (j = 0; j < x.length(); j++) {
                double temp = 0;
                double temp2 = 0;
                double tempTotalGrad = 0;

                for (int indexPoint = 0; indexPoint != aimStructure->size(); indexPoint++) {
                    temp = 0;
                    temp2 = 0;
                    double contri = 0;
                    double wei = 0;
                    unsigned int position = (*aimStructure)[indexPoint];

                    if ((*vvDij)[position].size() > j) {
                        contri = (*vvDij)[position][j];
                    }

                    if (contri > 0.0000000001) {
                        temp2 = 2 * contri * x[j];
                        temp = pow((*LBFGSopt::g_optData->getDose())[position], a - 1);
                        //tempTotalGrad += 2*aimEUD*aimEUD*temp/sumPowDose*temp2;

                        tempTotalGrad += aimEUD * temp / sumPowDose*temp2;

                    }
                }
                //cout<<tempTotalGrad<<" ";
                grad[j] += (tempTotalGrad) * aimWeight;

            }

        } else {//for uniform upper lower constraints

            double tempTotalFunc = 0;
            int indexPoint = 0;
#ifdef USE_OMP
#pragma omp parallel for default(shared) private(indexPoint) reduction(+:tempTotalFunc)
#endif
            for (indexPoint = 0; indexPoint < aimStructure->size(); indexPoint++) {
                double temp = 0;
                double contri = 0;
                double wei = 0;
                double tempFunc = 0;
                unsigned int position = (*aimStructure)[indexPoint];

                temp = (*LBFGSopt::g_optData->getDose())[position];

                if ((constraintType == "upper")&&(temp > aimDose)) {
                    tempFunc = (temp - aimDose)*(temp - aimDose);
                } else if ((constraintType == "lower")&&(temp < aimDose)) {
                    tempFunc = (temp - aimDose)*(temp - aimDose);
                } else if ((constraintType == "uniform")) {
                    tempFunc = (temp - aimDose)*(temp - aimDose);
                }

                tempTotalFunc += tempFunc;
            }

            func += (tempTotalFunc / double(aimStructure->size())) * aimWeight;

            int j = 0;
#ifdef USE_OMP
#pragma omp parallel for default(shared) private(j,indexPoint) schedule(dynamic)
#endif
            for (j = 0; j < x.length(); j++) {
                double tempTotalGrad = 0;

                for (indexPoint = 0; indexPoint < aimStructure->size(); indexPoint++) {
                    double temp = 0;
                    double temp2 = 0;
                    double contri = 0;
                    double wei = 0;
                    unsigned int position = (*aimStructure)[indexPoint];

                    if ((*vvDij)[position].size() > j) {
                        contri = (*vvDij)[position][j];
                    }

                    if (contri > 0.0000000001) {
                        temp2 = contri * x[j];

                        temp = (*LBFGSopt::g_optData->getDose())[position];

                        if ((constraintType == "upper")&&(temp > aimDose)) {
                            tempTotalGrad += 4 * (temp - aimDose) * temp2;
                        } else if ((constraintType == "lower")&&(temp < aimDose)) {
                            tempTotalGrad += 4 * (temp - aimDose) * temp2;
                        } else if ((constraintType == "uniform")) {
                            tempTotalGrad += 4 * (temp - aimDose) * temp2;
                        }
                    }
                }
                grad[j] += (tempTotalGrad / double(aimStructure->size())) * aimWeight;
            }
        }
    }

    cout << func << endl;
    g_objFunValue->push_back(func);
}

void LBFGSopt::optimization_EUD(vector<double> *resutlWeight, OptData* optData, vector<SConstraint> *pConstrain, vector<double> *objFunValue, int maxIter) {
    LBFGSopt::g_optData = optData;
    LBFGSopt::g_pConstraints = pConstrain;
    LBFGSopt::g_objFunValue = objFunValue;

    unsigned int nWeightLen = LBFGSopt::g_optData->getWeightTotal()->size();
    //size_t xLen = g_pDoseMatrix->m_vWeightUni->size();
    //initialize weight
    //make sure all beamlets used in optimization are != 0
    //since if beamlet weight == 0, it will always == 0 
    vector<double> tempWeight;
    tempWeight.resize(nWeightLen, 0);
    for (int i = 0; i < optData->getDijTotal()->size(); i++) {
        for (int j = 0; j < (*optData->getDijTotal())[i].size(); j++) {
            if ((*optData->getDijTotal())[i][j] > 0) {
                tempWeight[j] += (*optData->getDijTotal())[i][j];
            }
        }
    }

    double *xArray = new double[nWeightLen];
    for (int indexWei = 0; indexWei != nWeightLen; indexWei++) {
        if ((*LBFGSopt::g_optData->getWeightTotal())[indexWei] > 0) {
            xArray[indexWei] = sqrt((*LBFGSopt::g_optData->getWeightTotal())[indexWei]);
        } else if (tempWeight[indexWei] > 0) {
            xArray[indexWei] = 0.00001;
        } else {
            xArray[indexWei] = 0;
        }

    }

    real_1d_array x;
    x.setcontent(nWeightLen, xArray);
    delete xArray;

    double epsg = 0.0000000001;
    double epsf = 0;
    double epsx = 0;
    ae_int_t maxits = maxIter;
    minlbfgsstate state;
    minlbfgsreport rep;

    minlbfgscreate(1, x, state);
    minlbfgssetcond(state, epsg, epsf, epsx, maxits);

    alglib::minlbfgsoptimize(state, LBFGSopt::function_grad_EUD_new);

    minlbfgsresults(state, x, rep);

    for (int i = 0; i < x.length(); i++) {
        resutlWeight->push_back(x[i] * x[i]);
    }
}